package com.sholmes.android.scribbler;

import java.io.IOException;
import java.util.UUID;

import android.util.Log;

@SuppressWarnings("unused")
public class Scribbler implements Robot {
	private String TAG="Scribbler";
	
	  //Unique UUID for this application
    public static final UUID SCRIBBLER_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
    
	//TODO: Add different byte codes to control the Scribbler Robot.
	/*
	 * The following byte code values are from the myro scribbler.py source.
	 * see http://svn.cs.brynmawr.edu/viewvc/Myro/trunk/myro/robots/scribbler.py
	 */
	private int SOFT_RESET 			= 33;
	private int GET_ALL				= 65;
	private int GET_ALL_BINARY		= 66;
	private int GET_LIGHT_LEFT		= 67;
	private int GET_LIGHT_CENTER	= 68;
	private int GET_LIGHT_RIGHT		= 69;
	private int GET_LIGHT_ALL		= 70;
	private int GET_IR_LEFT			= 71;
	private int GET_IR_RIGHT		= 72;
	private int GET_IR_ALL			= 73;
	private int GET_LINE_LEFT		= 74;
	private int GET_LINE_RIGHT		= 75;
	private int GET_LINE_ALL		= 76;
	private int GET_STATE			= 77;
	private int GET_NAME1			= 78;
	private int GET_NAME2			= 79;
	private int GET_STALL			= 79;
	private int GET_INFO			= 80;
	private int GET_DATA			= 81;
	
	private int GET_PASS1			= 50;
	private int GET_PASS2			= 51;
	
	private int GET_RLE				= 82;	// A segmented and run-length encoded image
	private int GET_IMAGE			= 83;	// The entire 256x192 image in YUYV format
	private int GET_WINDOW			= 84;	// The windowed image (followed by which window)
	private int GET_DONGLE_L_IR		= 85;	// Number of returned pulses when left emitter is turned on
	private int GET_DONGLE_C_IR		= 86;	// Number of returned pulses when center emitter is turned on
	private int GET_DONGLE_R_IR		= 87;	// Number of returned pulses when right emitter is turned on
	private int GET_WINDOW_LIGHT	= 88;	// Average intensity in the user defined region 
	private int GET_BATTERY			= 89;	// Battery Voltage
	private int GET_SERIAL_MEM		= 90;	// With the address returns the value in serial memory
	private int GET_SCRIB_PROGRAM 	= 91;	// With offset, returns the scribbler program buffer
	private int GET_CAM_PARAM		= 92;	// With address returns the camera parameter at that address
	
	private int GET_BLOB			= 95;
	
	private int SET_PASS1			= 55;
	private int SET_PASS2			= 56;
	
	private int SET_SINGLE_DATA		= 96;
	private int SET_DATA			= 97;
	private int SET_ECHO_MODE		= 98;
	
	private int SET_LED_LEFT_ON		= 99;
	private int SET_LED_LEFT_OFF	= 100;
	private int SET_LED_CENTER_ON	= 101;
	private int SET_LED_CENTER_OFF	= 102;
	private int SET_LED_RIGHT_ON	= 103;
	private int SET_LED_RIGHT_OFF	= 104;
	private int SET_LED_ALL_ON		= 105;
	private int SET_LED_ALL_OFF		= 106;
	private int SET_LED_ALL			= 107;
	private int SET_MOTORS_OFF		= 108;
	private int SET_MOTORS			= 109;
	private int SET_NAME1			= 110;
	private int SET_NAME2			= 119;
	private int SET_LOUD			= 111;
	private int SET_QUIET			= 112;
	private int SET_SPEAKER			= 113;
	private int SET_SPEAKER_2		= 114;
	
	private int SET_DONGLE_LED_ON	= 116;	// turn binary dongle off
	private int SET_DONGLE_LED_OFF	= 117;	// turn binary dongle led off
	private int SET_RLE				= 118;	// Set rlf parameters 
	private int SET_DONGLE_IR		= 120;	// Set Dongle IR Power
	private int SET_SERIAL_MEM		= 121;
	private int SET_SCRIB_PROGRAM	= 122;
	private int SET_START_PROGRAM	= 123;
	private int SET_RESET_SCRIBBLER = 124;
	private int SET_SERIAL_ERASE	= 125;
	private int SET_DIMMER_LED		= 126;
	private int SET_WINDOW			= 127;
	private int SET_FORWARDNESS		= 128;
	private int SET_WHITE_BALANCE	= 129;
	private int SET_NO_WHITE_BALANCE= 130;
	private int SET_CAM_PARAM		= 131;
	
	private int GET_JPEG_GRAY_HEADER= 135;
	private int GET_JPEG_COLOR_HEADER=137;
	private int GET_JPEG_COLOR_SCAN	= 138;
	private int SET_PASS_N_BYTES	= 139;
	
	
	
	
	
	private int MIN_FORWARD_VALUE 	= 100;
	private int MIN_REVERSE_VALUE 	= 99;
	private int STOP_VALUE 			= 100;
	
	
	/*
	 * Need to determine if we are using the Fluke board or the standard scribbler bluetooth module.
	 * if using the fluke board we need to use the passthrough  creating wrapper packets  
	 * 
	 * 
	 */
	
	
	
	/**
	 * 
	 * 
	 */
	public Scribbler(){		
		
	}
		

	
	
	/**
	 * Crazy check 
	 * @param packet
	 * @param size
	 * @return true if packet size is as expected.
	 * @throws Exception
	 */
	//TODO: Include custom exception for invalid packet size
	private boolean packetCheck(byte[] packet, int size) throws Exception{
		if(packet.length == size){
			return true;
		}
		else{
			throw new Exception("Packet size is unexpected (change batteries): " + size + ":" +packet.length);					
		}		
	}
		
	
	/**
	 * Creates a packet to send to the robot  turning 180 Degrees
	 */
	@Override
	public byte[] turnAround() throws IOException {
		//TODO: call forward with the correct values to rotate the robot 180 degrees
		return null;
	}

	/**
	 * Creates a packet to send to the robot  turning 90 Degrees to the right
	 */
	@Override
	public byte[] turnLeft() throws IOException {
		//TODO: call forward with the correct values to rotate the robot 90degrees to the left
		return null;
	}

	/**
	 * Creates a packet to send to the robot  turning 90 Degrees to the right 
	 */
	@Override
	public byte[] turnRight() throws Exception {
		//TODO: call forward with the correct values to rotate the robot 90degrees to the right
		return null;
	}

	/**
	 * Creates a packet to send to the robot setting setting the motors in to stop values
	 * 
	 * @throws Exception 
	 */
	@Override
	public byte[] halt() throws Exception {
		Log.d(TAG, "Stop");		
		//Build packet to send to scribbler 
		byte stop[] = {(byte)(SET_MOTORS & 0xff),(byte)(STOP_VALUE & 0xff),(byte)(STOP_VALUE & 0xff),0,0,0,0,0,0};		
		
		//Check to see if packet is valid
		if(!packetCheck(stop,9)){
			Log.e(TAG, "Stop packet is not of expect size");
		}		
		return stop;		
	}

	/**
	 * Creates a packet to send to the robot setting its direction and speed backwards
	 * 
	 * @param speed
	 * @param direction
	 * @throws Exception
	 */
	@Override
	public byte[] backward(Integer speed, Float direction) throws Exception {
		//TODO: Currently do not use values and just run robot full speed backwards
		byte backward[] = { (byte)(109 & 0xff),0,0,0,0,0,0,0,0};
		
		
		
		//Check to verify the packet is valid.  If is not expected size packetCheck throws an Exception
		if(!packetCheck(backward,9)){
			Log.e(TAG, "Backward packet is not of expect size: " + backward.length);
		}
		
		return backward;
	}

	
	/**
	 * Creates a packet to send to the robot setting its direction and speed forwards 
	 * 
	 * @param speed
	 * @param direction
	 * @throws Exception
	 */
	@Override
	public byte[] forward(Integer speed, Float direction) throws Exception {
//		Log.d(TAG, "Forward: " + speed + " speed / " + direction + "degrees from 0");
//		byte[] forward = new byte[9];
//		forward[0] = (byte) ((SET_MOTORS) & 0xff);
//		for(int i=1; i <= 8; i++ ){
//			forward[i] = 0;
//		}
//		
//		if(direction > 0){	//rotate right			
//			forward[1] = (byte)((speed * 10 + MIN_FORWARD_VALUE - direction.intValue()) & 0xff);
//			forward[2] = (byte)((speed * 10 + MIN_FORWARD_VALUE) & 0xff);					
//		}else{				//Rotate left or straight
//			forward[1] = (byte)((speed * 10 + MIN_FORWARD_VALUE) & 0xff);
//			forward[2] = (byte)((speed * 10 + MIN_FORWARD_VALUE - direction.intValue()) & 0xff);
//		}
//		
		byte[] forward = {(byte)(109 & 0xff),(byte)(150 & 0xff),(byte)(150 & 0xff),0,0,0,0,0,0};
		//Check to verify the packet is valid.  If is not expected size packetCheck throws an Exception
		if(!packetCheck(forward,9)){
			Log.e(TAG, "Forward packet is not of expect size: " + forward.length);
		}
		
		return forward;		
	}	
}
